#include "AStarAlgorithm.h"
#include <iostream>
// #include <random>
#include <time.h>
#include <queue>
#include <set>

using namespace std;

const int X_COUNT = 40, Y_COUNT = 10;
const int NONE_POINT = 0;
const int BEGIN_POINT = 1;
const int END_POINT = 2;
const int FENCE_POINT = 3;
const int ROAD_1 = 4;
const int ROAD_2 = 5;
const int ROAD_3 = 6;
const int ROAD_4 = 7;
const int ROAD_5 = 8;
const int ROAD_6 = 9;

struct AStarPoint
{
    friend ostream &operator<<(ostream &out, const AStarPoint &other);

    // 坐标
    int x, y;
    // 当前代价：从起点到当前点已付出的代价
    int costF = 0;
    // 预估代价：当前点到终点预计需要的代价
    int costG = 0;
    AStarPoint *parent;

    AStarPoint() : AStarPoint(0, 0, nullptr){};
    AStarPoint(int x, int y) : AStarPoint(x, y, nullptr){};
    AStarPoint(int x, int y, AStarPoint *parent) : x(x), y(y), parent(parent) {}

    void calculateCostF(const AStarPoint point)
    {
        // point 传递的是上一个点到当前点的话只需要加 1
        costF = point.costF + 1;
        // point 传入的是起点的话，可以计算当前点到起点的曼哈顿距离：
        // costF = abs(x - point.x) + abs(y - point.y);
    }

    void calculateCostG(const AStarPoint point)
    {
        /*
                             曼哈顿距离
                           ↙
                    o────┐
                      \  |
                    ┌─>\ |
                    |   \|
                    |    x
                  欧拉距离
        */

        // 欧拉距离计算代价（两点之间直线距离）
        // (x1 - x2)² + (y1 - y2)²
        // costG = (x - point.x) * (x - point.x) + (y - point.y) * (y - point.y);

        // 曼哈顿距离计算代价（两点之间 x 方向与 y 方向的距离之和）
        // |x1 - x2| + |y1 - y2|
        costG = abs(x - point.x) + abs(y - point.y);
    }

    // 总代价：当前已付出的代价 + 预估还需要的代价
    int cost() const
    {
        return costF + costG;
    }

    bool operator==(const AStarPoint &other) const
    {
        return x == other.x && y == other.y;
    }

    bool operator!=(const AStarPoint &other) const
    {
        return !(*this == other);
    }

    // 重载小于运算符
    bool operator<(const AStarPoint &other) const
    {
        if (x == other.x)
        {
            return y < other.y; // 如果x相等，则比较y
        }
        return x < other.x; // 否则，比较x
    }
};

ostream &operator<<(ostream &out, const AStarPoint &other)
{
    out << "{x:" << other.x << ", y:" << other.y << ", costG:" << other.costG << ", costF:" << other.costF << ", cost:" << other.cost() << "}";
    return out;
}

struct AStarPointCompare
{
    bool operator()(const AStarPoint &lhs, const AStarPoint &rhs) const
    {
        return lhs.cost() > rhs.cost();
    }
};

vector<AStarPoint> aStarAlgorithm(const int (&data)[Y_COUNT][X_COUNT], AStarPoint &&begin_point, AStarPoint &&end_point)
{
    // 定义一个集合来记录已经被考察过了的节点。
    set<AStarPoint> closed_set;
    // 定义一个优先排序队列来记录需要进一步进行考察的节点，按照总代价来排序
    // 判定代价优先级的应该是独立一个算法，这里使用 AStarPointCompare 来作为队列判定条件函数
    // 在这里我第一次搞混了吧 AStarPoint 判定的操作符重载当作判断它优先级的条件，这导致很多问题，
    // 应该分开来看，判定考察优先级的应该不属于 AStarPoint 本身，AStarPoint 只需要管好自己，而考察优先级判定应该又另外的算法来处理。
    priority_queue<AStarPoint, vector<AStarPoint>, AStarPointCompare> open_queue;
    // 定义一个 vector 来保存最终的寻址路径节点
    vector<AStarPoint> path;

    // 计算起点的预估代价
    begin_point.calculateCostG(end_point);
    cout << "起点的总代价:" << begin_point.cost() << endl;

    // 从起点开始考察，所以将起点放入考察队列
    open_queue.push(begin_point);

    while (!open_queue.empty())
    {
        // 寻找代价最低的线路节点进行：代价即移动步数
        // open_queue 是一个 priority_queue 有限队列，会将元素在插入时自动排序，围绕着总成本进行优先排序，那么每次取出 top 都应该时最少代价节点
        // 为什么需要使用 open_queue 来存储：
        //  * 如果没有障碍物，那么可以沿着记录每次最少代价的节点查询下去，也就不需要队列来存储
        //  * 如果存在障碍物，那么有可能打断当前最少代价的节点线路查询，线路终断了就失败了，所以我们需要记录下查找的节点来返回另一条可能继续的最少代价线路，如何查找：
        //    - 我们每一次从已打开的队列 open_queue 中取出最优节点，同时我们就会弹出最优的线路节点这样队列中只会保存未曾走过的线路节点
        //    - 由于是排序队列，每次 top 取出的都会时最小代价节点，但取过的会被踢出保证不会重新取，而不符合条件的也不会被添加到队列，所以当遇到障碍无法在取，那么返回的
        //      就会是 queue 里其他代价最小的那条线路重新开始
        // open_queue 可以使用任何可排序的容器来代替，或者，自己写排序算法来规整，这里 使用标准容器减少相关代码。
        AStarPoint current_point = open_queue.top();
        open_queue.pop();

        // 考察的目的是发现考察的节点是否是终点，如果不是，就将考察节点的周围节点作为下一个考察目标。
        // 当前判定是否为终点，不是则执行后面代码去手机考察节点周围符合条件的待考察节点，排除不可选取（障碍等）和越界节点。
        // 并在一个优先队列中，以节点移动到终点的总代价为权重排序，这样考察队列里虽然存放了周围所有的节点，但下一次考察取出的节点任然是最小代价优先考察。
        if (current_point == end_point)
        {
            AStarPoint temp = *current_point.parent;
            while (temp.parent != nullptr)
            {
                path.push_back(temp);
                temp = *temp.parent;
            }

            // path.push_back(begin_point);
            return path;
        }

        // 将当前考察的节点放入已考察过的集合里
        closed_set.insert(current_point);

        // 定义遍历的邻居节点
        AStarPoint neighbors[] = {
            AStarPoint(current_point.x, current_point.y - 1), // 上
            AStarPoint(current_point.x + 1, current_point.y), // 左
            AStarPoint(current_point.x, current_point.y + 1), // 下
            AStarPoint(current_point.x - 1, current_point.y), // 右
        };

        // 不写死长度是因为目前是 4 向搜索，之后可以改成 8 向搜索
        // 那么查找遍历的节点能是 8 个方向，动态计算长度可以避免后面程序的调整只需要增加方向
        const int neighbors_length = sizeof(neighbors) / sizeof(AStarPoint);

        for (int i = 0; i < neighbors_length; ++i)
        {
            AStarPoint &neighbor_point = neighbors[i];

            // 判定考察节点并没有越界，且考察节点不属于障碍物，当然如果障碍物有多个，可以修改为考察节点不为空地类型。
            if (neighbor_point.x < 0 || neighbor_point.y < 0 ||
                neighbor_point.x >= X_COUNT || neighbor_point.y >= Y_COUNT ||
                data[neighbor_point.y][neighbor_point.x] == FENCE_POINT)
            {
                continue;
            }

            // 如果 neighbor_point 节点不在已考察过的集合里，那么就可以进一步考察
            if (closed_set.find(neighbor_point) == closed_set.end())
            {
                // 计算当前代价
                neighbor_point.calculateCostF(current_point);
                // 计算预估代价
                neighbor_point.calculateCostG(end_point);
                // 指明当前节点的来时方向
                // TODO: 这里需要优化的，这个地方的内存并未释放会造成内存泄漏，并且，当前算法内的 AStarPoint 都是拷贝对象，可以使用指针提升性能，并且同一管理指针避免内存泄漏
                neighbor_point.parent = new AStarPoint(current_point.x, current_point.y, current_point.parent);
                // 将符合条件的放入待考察队列
                open_queue.push(neighbor_point);

                // cout << current_point << " === " << neighbor_point.parent << " === " << *neighbor_point.parent << endl;
            }
        }
    }

    return path;
}

void showMapData(const int (&data)[Y_COUNT][X_COUNT])
{
    cout << "○：起点；x：终点；█：障碍；其他：线路；" << endl;
    for (int i = 0; i < X_COUNT; ++i)
    {
        cout << "▁"; // ─
    }
    cout << endl;

    for (int y = 0; y < Y_COUNT; ++y)
    {
        for (int x = 0; x < X_COUNT; ++x)
        {
            switch (data[y][x])
            {
            case NONE_POINT:
                cout << " ";
                break;
            case BEGIN_POINT:
                cout << "○";
                break;
            case END_POINT:
                cout << "x";
                break;
            case FENCE_POINT:
                cout << "█";
                break;
            case ROAD_1:
                cout << "─";
                break;
            case ROAD_2:
                cout << "│";
                break;
            case ROAD_3:
                cout << "┐";
                break;
            case ROAD_4:
                cout << "┘";
                break;
            case ROAD_5:
                cout << "┌";
                break;
            case ROAD_6:
                cout << "└";
                break;
            }
        }
        cout << "│" << y << endl;
    }

    for (int i = 0; i < X_COUNT; ++i)
    {
        cout << "▔";
    }
    cout << endl;
}

int randomRange(int begin, int end)
{
    // static default_random_engine randomEngine(time(0));
    // uniform_int_distribution<unsigned> uniformGenerator(begin, end);
    // return uniformGenerator(randomEngine);

    return (rand() % (end - begin + 1) + begin);
}

void aStarAlgorithmDemo()
{
    cout << "A-Star（A星）算法演示" << endl;

    srand(time(0));

    int data[Y_COUNT][X_COUNT];

    // 数组空间是连续分配的，所以二维数组的空间其实也是紧挨连续分配，不光是 fill 操作其他需要的如 find 等操作也可以利用这一点：
    // 我们可以获取导第一个数组的第一个元素和最后一个数组的最后一个元素作为迭代器范围来进行初始化
    fill(begin(data[0]), end(data[Y_COUNT - 1]), NONE_POINT);

    // 起点
    int begin_x = randomRange(0, X_COUNT - 1);
    int begin_y = randomRange(0, Y_COUNT - 1);
    cout << "起点：" << begin_x << "," << begin_y << endl;
    data[begin_y][begin_x] = BEGIN_POINT;

    // 终点
    int end_x = randomRange(0, X_COUNT - 1);
    int end_y = randomRange(0, Y_COUNT - 1);
    cout << "终点：" << end_x << "," << end_y << endl;
    data[end_y][end_x] = END_POINT;

    // 障碍
    int min_x = min<int>(begin_x, end_x);
    int min_y = min<int>(begin_y, end_y);
    int max_x = max<int>(begin_x, end_x);
    int max_y = max<int>(begin_y, end_y);

    if ((max_x - min_x) > (max_y - min_y))
    {
        int x = (end_x + begin_x) / 2;
        for (int i = 0; i < (max_y - min_y) + 4; ++i)
        {
            int y = min(max(0, min_y - 2 + i), Y_COUNT - 1);
            if (data[y][x] == NONE_POINT)
                data[y][x] = FENCE_POINT;
        }
    }
    else
    {
        int y = (end_y + begin_y) / 2;
        for (int i = 0; i < (max_x - min_x) + 4; ++i)
        {
            int x = min(max(0, min_x - 2 + i), X_COUNT - 1);
            if (data[y][x] == NONE_POINT)
                data[y][x] = FENCE_POINT;
        }
    }

    // 围住起点
    int around_x = end_x; // begin_x;
    int around_y = end_y; // begin_y;
    if (around_y - 1 > 0 && data[around_y - 1][around_x] == NONE_POINT)
        data[around_y - 1][around_x] = FENCE_POINT;
    if (around_x - 1 > 0 && data[around_y][around_x - 1] == NONE_POINT)
        data[around_y][around_x - 1] = FENCE_POINT;
    if (around_x + 1 < X_COUNT && data[around_y][around_x + 1] == NONE_POINT)
        data[around_y][around_x + 1] = FENCE_POINT;

    showMapData(data);

    // A-Star 算法寻路：核心看 aStarAlgorithm 的实现，这个函数的其他部分只是用于表现渲染的代码并不牵涉算法核心。
    vector<AStarPoint> path = aStarAlgorithm(data, {begin_x, begin_y}, {end_x, end_y});

    // for(auto it = path.begin(); it != path.end(); ++it) {
    //     AStarPoint& point = *it;
    //     data[point.y][point.x] = 1;
    // }
    int last_x = begin_x;
    int last_y = begin_y;
    for (int i = path.size() - 1; i >= 0; --i)
    {
        AStarPoint &point = path[i];
        // cout << point << endl;

        int next_x = (i - 1) < 0 ? end_x : path[i - 1].x;
        int next_y = (i - 1) < 0 ? end_y : path[i - 1].y;

        if (last_y == next_y)
        {
            data[point.y][point.x] = ROAD_1; // ─
        }
        else if (last_x == next_x)
        {
            data[point.y][point.x] = ROAD_2; // │
        }
        else if ((last_x < point.x && point.y < next_y) || (last_y > point.y && point.x > next_x))
        {
            data[point.y][point.x] = ROAD_3; // ┐
        }
        else if ((last_x < point.x && point.y > next_y) || (last_y < point.y && point.x > next_x))
        {
            data[point.y][point.x] = ROAD_4; // ┘
        }
        else if ((last_y > point.y && point.x < next_x) || (last_x > point.x && point.y < next_y))
        {
            data[point.y][point.x] = ROAD_5; // ┌
        }
        else if ((last_y < point.y && point.x < next_x) || (last_x > point.x && point.y > next_y))
        {
            data[point.y][point.x] = ROAD_6; // └
        }
        last_x = point.x;
        last_y = point.y;
    }

    showMapData(data);
}